﻿
// 2019/4/11: 首个记录

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using ASEva;

namespace GlobalObjectsSampleV1
{
    enum DynamicObjectType
    {
        Unknown = 0,
        Car = 1, // 轿车
        Van = 2, // 面包车
        Truck = 3, // 火车
        Trailer = 4, // 拖车
        Semitrailer = 5, // 货柜车
        Bus = 6, // 大巴
        Motorbike = 7, // 摩托车
        Bicycle = 8, // 自行车
        Train = 9, // 火车
        Tram = 10, // 有轨电车
        Pedestrian = 11, // 行人
        Wheelchair = 12, // 轮椅
        Animal = 13, // 动物
    }

    class DynamicObject
    {
        public int ID { get; set; } // 目标物ID，0表示为主目标物，1~表示一般目标物
        public DynamicObjectType Type { get; set; } // 目标物类型
        public double CenterLongitude { get; set; } // 目标物中心的经度，东为正方向
        public double CenterLatitude { get; set; } // 目标物中心的纬度，北为正方向
        public double? CenterAltitude { get; set; } // 目标物地面的海拔，单位m
        public double? Orientation { get; set; } // 目标物朝向，北为0，逆时针为正方向
        public double? Width { get; set; } // 目标物宽度
        public double? Length { get; set; } // 目标物长度
        public double? Height { get; set; } // 目标物高度
        public double? Speed { get; set; } // 目标物速度，KPH
    }

    class GlobalObjectsSample : Sample
    {
        public List<DynamicObject> Objects { get; set; } // 目标物列表

        public GlobalObjectsSample()
        {
            Objects = new List<DynamicObject>();
        }

        public static string Protocol
        {
            get
            {
                return "global-objects-sample-v1";
            }
        }
        public static string[] Protocols
        {
            get
            {
                return new String[] { "global-objects-sample-v1" };
            }
        }
        public static string Title
        {
            get
            {
                return "Object count,First object's ID,First object's type,First object's center longitude[°E],First object's center latitude[°N],First object's ground altitude[m],First object's orientation[°],First object's width[m],First object's length[m],First object's height[m],First object's speed[KPH]";
            }
        }
        public override string GetGeneralSampleProtocol()
        {
            return Protocol;
        }
        public override string[] GetGeneralSampleProtocols()
        {
            return Protocols;
        }
        public override bool SupportInterpolation()
        {
            return true;
        }
        public override bool FromGeneralSample(GeneralSample sample)
        {
            if (sample.Protocol == "global-objects-sample-v1")
            {
                if (sample.Values.Count < 1) return false;
                if (!sample.Values[0].IsNumber()) return false;

                int objectCount = sample.Values[0].ToInt().Value;
                if (sample.Values.Count != 1 + 10 * objectCount) return false;

                Objects = new List<DynamicObject>();
                int baseIndex = 1;
                for (int i = 0; i < objectCount; i++)
                {
                    var obj = new DynamicObject();
                    obj.ID = sample.Values[baseIndex + 0].ToInt().Value;
                    obj.Type = (DynamicObjectType)sample.Values[baseIndex + 1].ToInt().Value;
                    obj.CenterLongitude = sample.Values[baseIndex + 2].ToDouble().Value;
                    obj.CenterLatitude = sample.Values[baseIndex + 3].ToDouble().Value;
                    obj.CenterAltitude = sample.Values[baseIndex + 4].ToDouble();
                    obj.Orientation = sample.Values[baseIndex + 5].ToDouble();
                    obj.Width = sample.Values[baseIndex + 6].ToDouble();
                    obj.Length = sample.Values[baseIndex + 7].ToDouble();
                    obj.Height = sample.Values[baseIndex + 8].ToDouble();
                    obj.Speed = sample.Values[baseIndex + 9].ToDouble();
                    Objects.Add(obj);

                    baseIndex += 10;
                }

                Base = sample.Base;
                Offset = sample.Offset;
                Timeline = sample.Timeline;

                return true;
            }

            return false;
        }
        public override GeneralSample ToGeneralSample()
        {
            var sample = new GeneralSample();
            sample.Base = Base;
            sample.Offset = Offset;
            sample.Timeline = Timeline;
            sample.Protocol = GetGeneralSampleProtocol();

            var data = new GeneralSampleValue[1 + 10 * Objects.Count];
            sample.NumberOfSignificants = data.Length;

            data[0] = new GeneralSampleValue((double)Objects.Count);

            int baseIndex = 1;
            for (int i = 0; i < Objects.Count; i++)
            {
                var obj = Objects[i];
                data[baseIndex + 0] = new GeneralSampleValue((double)obj.ID);
                data[baseIndex + 1] = new GeneralSampleValue((double)(uint)obj.Type);
                data[baseIndex + 2] = new GeneralSampleValue(obj.CenterLongitude);
                data[baseIndex + 3] = new GeneralSampleValue(obj.CenterLatitude);
                if (obj.CenterAltitude != null) data[baseIndex + 4] = new GeneralSampleValue(obj.CenterAltitude.Value);
                if (obj.Orientation != null) data[baseIndex + 5] = new GeneralSampleValue(obj.Orientation.Value);
                if (obj.Width != null) data[baseIndex + 6] = new GeneralSampleValue(obj.Width.Value);
                if (obj.Length != null) data[baseIndex + 7] = new GeneralSampleValue(obj.Length.Value);
                if (obj.Height != null) data[baseIndex + 8] = new GeneralSampleValue(obj.Height.Value);
                if (obj.Speed != null) data[baseIndex + 9] = new GeneralSampleValue(obj.Speed.Value);

                baseIndex += 10;
            }

            sample.Values = data.ToList();

            return sample;
        }
        protected override Sample Interpolate(Sample.SearchResult input)
        {
            var s1 = input.s1 as GlobalObjectsSample;
            var s2 = input.s2 as GlobalObjectsSample;
            var w1 = input.w1;
            var w2 = input.w2;

            var output = new GlobalObjectsSample();
            foreach (var s1Obj in s1.Objects)
            {
                foreach (var s2Obj in s2.Objects)
                {
                    if (s1Obj.ID == s2Obj.ID)
                    {
                        var obj = new DynamicObject();
                        obj.ID = s1Obj.ID;
                        obj.Type = w1 > w2 ? s1Obj.Type : s2Obj.Type;
                        obj.CenterLongitude = s1Obj.CenterLongitude * w1 + s2Obj.CenterLongitude * w2;
                        obj.CenterLatitude = s1Obj.CenterLatitude * w1 + s2Obj.CenterLatitude * w2;
                        obj.CenterAltitude = s1Obj.CenterAltitude * w1 + s2Obj.CenterAltitude * w2;
                        obj.Orientation = interpolateAngle(s1Obj.Orientation, w1, s2Obj.Orientation, w2);
                        obj.Width = s1Obj.Width * w1 + s2Obj.Width * w2;
                        obj.Length = s1Obj.Length * w1 + s2Obj.Length * w2;
                        obj.Height = s1Obj.Height * w1 + s2Obj.Height * w2;
                        obj.Speed = s1Obj.Speed * w1 + s2Obj.Speed * w2;
                        output.Objects.Add(obj);
                        break;
                    }
                }
            }

            return output;
        }

        private double? interpolateAngle(double? a1, double w1, double? a2, double w2)
        {
            if (a1 == null || a2 == null) return null;

            var deg2rad = Math.PI / 180;
            var x1 = Math.Cos(a1.Value * deg2rad);
            var y1 = Math.Sin(a1.Value * deg2rad);
            var x2 = Math.Cos(a2.Value * deg2rad);
            var y2 = Math.Sin(a2.Value * deg2rad);

            var xo = x1 * w1 + x2 * w2;
            var yo = y1 * w1 + y2 * w2;
            if (xo == 0 && yo == 0) return null;

            return Math.Atan2(yo, xo) / deg2rad;
        }
    }
}
